#include <Matrix.h>

#include "Utils.h"

#include "CoordDrawer.h"

CoordDrawer::CoordDrawer(QWidget *parent)
: QGLWidget(parent)
, _mode(0)
{
	setAutoBufferSwap(false);
}

CoordDrawer::CoordDrawer(const QGLFormat &fmt, QWidget *parent, int mode)
: QGLWidget(fmt, parent)
, _mode(mode)
{
	setAutoBufferSwap(false);
}

void CoordDrawer::setStabilization(const QQuaternion &q)
{
	_stabilization = q;
	updateGL();
}

void CoordDrawer::setQuat(const QQuaternion &q)
{
	_quat = q;
	updateGL();
}

void CoordDrawer::initializeGL()
{
}

void CoordDrawer::resizeGL(int w, int h)
{
	glViewport(0, 0, (GLint)w, (GLint)h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(35, double(w) / h, 0.1, 1000.0);
	glMatrixMode(GL_MODELVIEW);
}

void CoordDrawer::paintGL()
{
	if(format().stereo())
	{
		if(format().doubleBuffer())
		{
			drawScene(GL_BACK_LEFT);
			drawScene(GL_BACK_RIGHT);
		}
		else
		{
			drawScene(GL_FRONT_LEFT);
			drawScene(GL_FRONT_RIGHT);
		}
	}
	else
	{
		if(format().doubleBuffer())
		{
			drawScene(GL_BACK);
		}
		else
		{
			drawScene(GL_FRONT);
		}
	}

	if(format().doubleBuffer())
	{
		swapBuffers();
	}
}

void CoordDrawer::drawScene(GLenum buffer)
{
	glDrawBuffer(buffer);

	glClearColor(0.0, 0.0, 0.0, 1.0);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	GLfloat MaterialAmbient[] = {0.5f, 0.5f, 0.5f, 1.0f};
	GLfloat MaterialDiffuse[] = {1.0f, 1.0f, 1.0f, 1.0f};
	GLfloat MaterialSpecular[] = {0.4f, 0.4f, 0.4f, 1.0f};
	GLfloat MaterialShininess[] = {10.0f};
	GLfloat AmbientLightPosition[] = {0.5f, 1.0f, 1.0f, 0.0f};
	GLfloat LightAmbient[] = {0.5f, 0.5f, 0.5f, 1.0f};

	glMaterialfv(GL_FRONT, GL_AMBIENT, MaterialAmbient);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, MaterialDiffuse);
	glMaterialfv(GL_FRONT, GL_SPECULAR, MaterialSpecular);
	glMaterialfv(GL_FRONT, GL_SHININESS, MaterialShininess);
	glLightfv(GL_LIGHT0, GL_POSITION, AmbientLightPosition);
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, LightAmbient);

	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_COLOR_MATERIAL);
	glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
	glShadeModel(GL_SMOOTH);

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	if(buffer == GL_LEFT ||
	   buffer == GL_FRONT_LEFT ||
	   buffer == GL_BACK_LEFT)
	{
		glTranslatef(-0.5, 0, 0);
	}
	else if(buffer == GL_RIGHT ||
		buffer == GL_FRONT_RIGHT ||
		buffer == GL_BACK_RIGHT)
	{
		glTranslatef(0.5, 0, 0);
	}

	glTranslatef(0, 0, -40);

	if(_mode == 0 || _mode == 2)
	{
		drawCoord(_quat, 1.0);
	}

	if(_mode != 1)
	{
		glDisable(GL_DEPTH_TEST);
	}

	if(_mode != 2)
	{
		drawCoord(_stabilization, _mode == 0 ? 0.15 : 1.0);
	}
}

void CoordDrawer::drawArrow()
{
	glPushMatrix();
		solidCylinder(1, 6, 8, 2);
		glTranslatef(0, 0, 6);
		solidCone(1.8, 5, 20, 5);
	glPopMatrix();
}

void CoordDrawer::setMode(int index)
{
	_mode = index;
	updateGL();
}

static void cvtQuatToOpenGLMatrix(const QQuaternion &q, GLfloat (&glmat)[16])
{
	Matrix mat = Matrix::rotate(Quat(q.x(), q.y(), q.z(), q.scalar()));
	for(int col = 0; col < 4; ++col)
	{
		for(int row = 0; row < 4; ++row)
		{
			glmat[col * 4 + row] = mat(col, row);
		}
	}
}

void CoordDrawer::drawCoord(const QQuaternion &q, float alpha)
{
	GLfloat m[16];

	cvtQuatToOpenGLMatrix(q, m);

	glPushMatrix();
		glMultMatrixf(m);

		glPushMatrix();
			glRotatef(90, 0, 1, 0);
			glColor4f(1, 0, 0, alpha);
			drawArrow();
		glPopMatrix();

		glPushMatrix();
			glRotatef(-90, 1, 0, 0);
			glColor4f(0, 1, 0, alpha);
			drawArrow();
		glPopMatrix();

		glPushMatrix();
			glColor4f(0, 0, 1, alpha);
			drawArrow();
		glPopMatrix();
	glPopMatrix();
}

